<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvsfm/laSBA/sba-1.6/sba_levmar_wrap.c</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00002"></a>00002 
<a name="l00003"></a>00003 
<a name="l00004"></a>00004 
<a name="l00005"></a>00005 
<a name="l00006"></a>00006 
<a name="l00007"></a>00007 
<a name="l00008"></a>00008 
<a name="l00009"></a>00009 
<a name="l00010"></a>00010 
<a name="l00011"></a>00011 
<a name="l00012"></a>00012 
<a name="l00013"></a>00013 
<a name="l00014"></a>00014 
<a name="l00015"></a>00015 
<a name="l00016"></a>00016 
<a name="l00017"></a>00017 
<a name="l00018"></a>00018 
<a name="l00019"></a>00019 
<a name="l00020"></a>00020 
<a name="l00021"></a>00021 
<a name="l00022"></a>00022 <span class="preprocessor">#include &lt;stdio.h&gt;</span>
<a name="l00023"></a>00023 <span class="preprocessor">#include &lt;stdlib.h&gt;</span>
<a name="l00024"></a>00024 <span class="preprocessor">#include &lt;string.h&gt;</span>
<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;math.h&gt;</span>
<a name="l00026"></a>00026 <span class="preprocessor">#include &lt;float.h&gt;</span>
<a name="l00027"></a>00027 
<a name="l00028"></a>00028 <span class="preprocessor">#include &quot;sba.h&quot;</span>
<a name="l00029"></a>00029 
<a name="l00030"></a>00030 
<a name="l00031"></a>00031 <span class="preprocessor">#define FABS(x)           (((x)&gt;=0)? (x) : -(x))</span>
<a name="l00032"></a>00032 <span class="preprocessor"></span>
<a name="l00033"></a>00033 <span class="keyword">struct </span>wrap_motstr_data_ {
<a name="l00034"></a>00034   void   (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata); <span class="comment">// Q</span>
<a name="l00035"></a>00035   void (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *Aij, <span class="keywordtype">double</span> *Bij, <span class="keywordtype">void</span> *adata); <span class="comment">// dQ/da, dQ/db</span>
<a name="l00036"></a>00036   <span class="keywordtype">int</span> cnp, pnp, mnp; <span class="comment">/* parameter numbers */</span>
<a name="l00037"></a>00037   <span class="keywordtype">void</span> *adata;
<a name="l00038"></a>00038 };
<a name="l00039"></a>00039 
<a name="l00040"></a>00040 <span class="keyword">struct </span>wrap_mot_data_ {
<a name="l00041"></a>00041   void   (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata); <span class="comment">// Q</span>
<a name="l00042"></a>00042   void (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *Aij, <span class="keywordtype">void</span> *adata); <span class="comment">// dQ/da</span>
<a name="l00043"></a>00043   <span class="keywordtype">int</span> cnp, mnp; <span class="comment">/* parameter numbers */</span>
<a name="l00044"></a>00044   <span class="keywordtype">void</span> *adata;
<a name="l00045"></a>00045 };
<a name="l00046"></a>00046 
<a name="l00047"></a>00047 <span class="keyword">struct </span>wrap_str_data_ {
<a name="l00048"></a>00048   void   (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata); <span class="comment">// Q</span>
<a name="l00049"></a>00049   void (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *Bij, <span class="keywordtype">void</span> *adata); <span class="comment">// dQ/db</span>
<a name="l00050"></a>00050   <span class="keywordtype">int</span> pnp, mnp; <span class="comment">/* parameter numbers */</span>
<a name="l00051"></a>00051   <span class="keywordtype">void</span> *adata;
<a name="l00052"></a>00052 };
<a name="l00053"></a>00053 
<a name="l00054"></a>00054 <span class="comment">/* Routines to estimate the estimated measurement vector (i.e. &quot;func&quot;) and</span>
<a name="l00055"></a>00055 <span class="comment"> * its sparse jacobian (i.e. &quot;fjac&quot;) needed by BA expert drivers. Code below</span>
<a name="l00056"></a>00056 <span class="comment"> * makes use of user-supplied functions computing &quot;Q&quot;, &quot;dQ/da&quot;, d&quot;Q/db&quot;,</span>
<a name="l00057"></a>00057 <span class="comment"> * i.e. predicted projection and associated jacobians for a SINGLE image measurement.</span>
<a name="l00058"></a>00058 <span class="comment"> * Notice also that what follows is two pairs of &quot;func&quot; and corresponding &quot;fjac&quot; routines.</span>
<a name="l00059"></a>00059 <span class="comment"> * The first is to be used in full (i.e. motion + structure) BA, the second in </span>
<a name="l00060"></a>00060 <span class="comment"> * motion only BA.</span>
<a name="l00061"></a>00061 <span class="comment"> */</span>
<a name="l00062"></a>00062 
<a name="l00063"></a>00063 <span class="comment">/* FULL BUNDLE ADJUSTMENT */</span>
<a name="l00064"></a>00064 
<a name="l00065"></a>00065 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in</span>
<a name="l00066"></a>00066 <span class="comment"> * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements</span>
<a name="l00067"></a>00067 <span class="comment"> * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted</span>
<a name="l00068"></a>00068 <span class="comment"> * projection of the i-th point on the j-th camera.</span>
<a name="l00069"></a>00069 <span class="comment"> * Caller supplies rcidxs and rcsubs which can be used as working memory.</span>
<a name="l00070"></a>00070 <span class="comment"> * Notice that depending on idxij, some of the hx_ij might be missing</span>
<a name="l00071"></a>00071 <span class="comment"> *</span>
<a name="l00072"></a>00072 <span class="comment"> */</span>
<a name="l00073"></a>00073 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_motstr_Qs(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata)
<a name="l00074"></a>00074 {
<a name="l00075"></a>00075   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00076"></a>00076   <span class="keywordtype">int</span> cnp, pnp, mnp; 
<a name="l00077"></a>00077   <span class="keywordtype">double</span> *pa, *pb, *paj, *pbi, *pxij;
<a name="l00078"></a>00078   <span class="keywordtype">int</span> n, m, nnz;
<a name="l00079"></a>00079   <span class="keyword">struct </span>wrap_motstr_data_ *wdata;
<a name="l00080"></a>00080   void (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *proj_adata);
<a name="l00081"></a>00081   <span class="keywordtype">void</span> *proj_adata;
<a name="l00082"></a>00082 
<a name="l00083"></a>00083   wdata=(<span class="keyword">struct </span>wrap_motstr_data_ *)adata;
<a name="l00084"></a>00084   cnp=wdata-&gt;cnp; pnp=wdata-&gt;pnp; mnp=wdata-&gt;mnp;
<a name="l00085"></a>00085   proj=wdata-&gt;proj;
<a name="l00086"></a>00086   proj_adata=wdata-&gt;adata;
<a name="l00087"></a>00087 
<a name="l00088"></a>00088   n=idxij-&gt;nr; m=idxij-&gt;nc;
<a name="l00089"></a>00089   pa=p; pb=p+m*cnp;
<a name="l00090"></a>00090 
<a name="l00091"></a>00091   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00092"></a>00092     <span class="comment">/* j-th camera parameters */</span>
<a name="l00093"></a>00093     paj=pa+j*cnp;
<a name="l00094"></a>00094 
<a name="l00095"></a>00095     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00096"></a>00096 
<a name="l00097"></a>00097     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00098"></a>00098       pbi=pb + rcsubs[i]*pnp;
<a name="l00099"></a>00099       pxij=hx + idxij-&gt;val[rcidxs[i]]*mnp; <span class="comment">// set pxij to point to hx_ij</span>
<a name="l00100"></a>00100 
<a name="l00101"></a>00101       (*proj)(j, rcsubs[i], paj, pbi, pxij, proj_adata); <span class="comment">// evaluate Q in pxij</span>
<a name="l00102"></a>00102     }
<a name="l00103"></a>00103   }
<a name="l00104"></a>00104 }
<a name="l00105"></a>00105 
<a name="l00106"></a>00106 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in</span>
<a name="l00107"></a>00107 <span class="comment"> * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00108"></a>00108 <span class="comment"> * The jacobian is returned in the order (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm),</span>
<a name="l00109"></a>00109 <span class="comment"> * where A_ij=dx_ij/db_j and B_ij=dx_ij/db_i (see HZ).</span>
<a name="l00110"></a>00110 <span class="comment"> * Caller supplies rcidxs and rcsubs which can be used as working memory.</span>
<a name="l00111"></a>00111 <span class="comment"> * Notice that depending on idxij, some of the A_ij, B_ij might be missing</span>
<a name="l00112"></a>00112 <span class="comment"> *</span>
<a name="l00113"></a>00113 <span class="comment"> */</span>
<a name="l00114"></a>00114 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_motstr_Qs_jac(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata)
<a name="l00115"></a>00115 {
<a name="l00116"></a>00116   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00117"></a>00117   <span class="keywordtype">int</span> cnp, pnp, mnp;
<a name="l00118"></a>00118   <span class="keywordtype">double</span> *pa, *pb, *paj, *pbi, *pAij, *pBij;
<a name="l00119"></a>00119   <span class="keywordtype">int</span> n, m, nnz, Asz, Bsz, ABsz, idx;
<a name="l00120"></a>00120   <span class="keyword">struct </span>wrap_motstr_data_ *wdata;
<a name="l00121"></a>00121   void (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *Aij, <span class="keywordtype">double</span> *Bij, <span class="keywordtype">void</span> *projac_adata);
<a name="l00122"></a>00122   <span class="keywordtype">void</span> *projac_adata;
<a name="l00123"></a>00123 
<a name="l00124"></a>00124   
<a name="l00125"></a>00125   wdata=(<span class="keyword">struct </span>wrap_motstr_data_ *)adata;
<a name="l00126"></a>00126   cnp=wdata-&gt;cnp; pnp=wdata-&gt;pnp; mnp=wdata-&gt;mnp;
<a name="l00127"></a>00127   projac=wdata-&gt;projac;
<a name="l00128"></a>00128   projac_adata=wdata-&gt;adata;
<a name="l00129"></a>00129 
<a name="l00130"></a>00130   n=idxij-&gt;nr; m=idxij-&gt;nc;
<a name="l00131"></a>00131   pa=p; pb=p+m*cnp;
<a name="l00132"></a>00132   Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
<a name="l00133"></a>00133 
<a name="l00134"></a>00134   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00135"></a>00135     <span class="comment">/* j-th camera parameters */</span>
<a name="l00136"></a>00136     paj=pa+j*cnp;
<a name="l00137"></a>00137 
<a name="l00138"></a>00138     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00139"></a>00139 
<a name="l00140"></a>00140     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00141"></a>00141       pbi=pb + rcsubs[i]*pnp;
<a name="l00142"></a>00142       idx=idxij-&gt;val[rcidxs[i]];
<a name="l00143"></a>00143       pAij=jac  + idx*ABsz; <span class="comment">// set pAij to point to A_ij</span>
<a name="l00144"></a>00144       pBij=pAij + Asz; <span class="comment">// set pBij to point to B_ij</span>
<a name="l00145"></a>00145 
<a name="l00146"></a>00146       (*projac)(j, rcsubs[i], paj, pbi, pAij, pBij, projac_adata); <span class="comment">// evaluate dQ/da, dQ/db in pAij, pBij</span>
<a name="l00147"></a>00147     }
<a name="l00148"></a>00148   }
<a name="l00149"></a>00149 }
<a name="l00150"></a>00150 
<a name="l00151"></a>00151 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points and the parameters of m cameras, compute in</span>
<a name="l00152"></a>00152 <span class="comment"> * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00153"></a>00153 <span class="comment"> * The jacobian is approximated with the aid of finite differences and is returned in the order</span>
<a name="l00154"></a>00154 <span class="comment"> * (A_11, B_11, ..., A_1m, B_1m, ..., A_n1, B_n1, ..., A_nm, B_nm),</span>
<a name="l00155"></a>00155 <span class="comment"> * where A_ij=dx_ij/da_j and B_ij=dx_ij/db_i (see HZ).</span>
<a name="l00156"></a>00156 <span class="comment"> * Notice that depending on idxij, some of the A_ij, B_ij might be missing</span>
<a name="l00157"></a>00157 <span class="comment"> *</span>
<a name="l00158"></a>00158 <span class="comment"> * Problem-specific information is assumed to be stored in a structure pointed to by &quot;dat&quot;.</span>
<a name="l00159"></a>00159 <span class="comment"> *</span>
<a name="l00160"></a>00160 <span class="comment"> * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,</span>
<a name="l00161"></a>00161 <span class="comment"> * the jacobian should be computed analytically</span>
<a name="l00162"></a>00162 <span class="comment"> */</span>
<a name="l00163"></a>00163 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_motstr_Qs_fdjac(
<a name="l00164"></a>00164     <span class="keywordtype">double</span> *p,                <span class="comment">/* I: current parameter estimate, (m*cnp+n*pnp)x1 */</span>
<a name="l00165"></a>00165     <span class="keyword">struct</span> sba_crsm *idxij,   <span class="comment">/* I: sparse matrix containing the location of x_ij in hx */</span>
<a name="l00166"></a>00166     <span class="keywordtype">int</span>    *rcidxs,           <span class="comment">/* work array for the indexes of nonzero elements of a single sparse matrix row/column */</span>
<a name="l00167"></a>00167     <span class="keywordtype">int</span>    *rcsubs,           <span class="comment">/* work array for the subscripts of nonzero elements in a single sparse matrix row/column */</span>
<a name="l00168"></a>00168     <span class="keywordtype">double</span> *jac,              <span class="comment">/* O: array for storing the approximated jacobian */</span>
<a name="l00169"></a>00169     <span class="keywordtype">void</span>   *dat)              <span class="comment">/* I: points to a &quot;wrap_motstr_data_&quot; structure */</span>
<a name="l00170"></a>00170 {
<a name="l00171"></a>00171   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj;
<a name="l00172"></a>00172   <span class="keywordtype">double</span> *pa, *pb, *paj, *pbi;
<a name="l00173"></a>00173   <span class="keyword">register</span> <span class="keywordtype">double</span> *pAB;
<a name="l00174"></a>00174   <span class="keywordtype">int</span> n, m, nnz, Asz, Bsz, ABsz;
<a name="l00175"></a>00175 
<a name="l00176"></a>00176   <span class="keywordtype">double</span> tmp;
<a name="l00177"></a>00177   <span class="keyword">register</span> <span class="keywordtype">double</span> d, d1;
<a name="l00178"></a>00178 
<a name="l00179"></a>00179   <span class="keyword">struct </span>wrap_motstr_data_ *fdjd;
<a name="l00180"></a>00180   void (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata);
<a name="l00181"></a>00181   <span class="keywordtype">double</span> *hxij, *hxxij;
<a name="l00182"></a>00182   <span class="keywordtype">int</span> cnp, pnp, mnp;
<a name="l00183"></a>00183   <span class="keywordtype">void</span> *adata;
<a name="l00184"></a>00184 
<a name="l00185"></a>00185   <span class="comment">/* retrieve problem-specific information passed in *dat */</span>
<a name="l00186"></a>00186   fdjd=(<span class="keyword">struct </span>wrap_motstr_data_ *)dat;
<a name="l00187"></a>00187   proj=fdjd-&gt;proj;
<a name="l00188"></a>00188   cnp=fdjd-&gt;cnp; pnp=fdjd-&gt;pnp; mnp=fdjd-&gt;mnp;
<a name="l00189"></a>00189   adata=fdjd-&gt;adata;
<a name="l00190"></a>00190 
<a name="l00191"></a>00191   n=idxij-&gt;nr; m=idxij-&gt;nc;
<a name="l00192"></a>00192   pa=p; pb=p+m*cnp;
<a name="l00193"></a>00193   Asz=mnp*cnp; Bsz=mnp*pnp; ABsz=Asz+Bsz;
<a name="l00194"></a>00194 
<a name="l00195"></a>00195   <span class="comment">/* allocate memory for hxij, hxxij */</span>
<a name="l00196"></a>00196   <span class="keywordflow">if</span>((hxij=malloc(2*mnp*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>)))==NULL){
<a name="l00197"></a>00197     fprintf(stderr, <span class="stringliteral">&quot;memory allocation request failed in sba_motstr_Qs_fdjac()!\n&quot;</span>);
<a name="l00198"></a>00198     exit(1);
<a name="l00199"></a>00199   }
<a name="l00200"></a>00200   hxxij=hxij+mnp;
<a name="l00201"></a>00201 
<a name="l00202"></a>00202     <span class="comment">/* compute A_ij */</span>
<a name="l00203"></a>00203     <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00204"></a>00204       paj=pa+j*cnp; <span class="comment">// j-th camera parameters</span>
<a name="l00205"></a>00205 
<a name="l00206"></a>00206       nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero A_ij, i=0...n-1 */</span>
<a name="l00207"></a>00207       <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj){
<a name="l00208"></a>00208         <span class="comment">/* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */</span>
<a name="l00209"></a>00209         d=(double)(SBA_DELTA_SCALE)*paj[jj]; <span class="comment">// force evaluation</span>
<a name="l00210"></a>00210         d=FABS(d);
<a name="l00211"></a>00211         <span class="keywordflow">if</span>(d&lt;SBA_MIN_DELTA) d=SBA_MIN_DELTA;
<a name="l00212"></a>00212         d1=1.0/d; <span class="comment">/* invert so that divisions can be carried out faster as multiplications */</span>
<a name="l00213"></a>00213 
<a name="l00214"></a>00214         <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00215"></a>00215           pbi=pb + rcsubs[i]*pnp; <span class="comment">// i-th point parameters</span>
<a name="l00216"></a>00216           (*proj)(j, rcsubs[i], paj, pbi, hxij, adata); <span class="comment">// evaluate supplied function on current solution</span>
<a name="l00217"></a>00217 
<a name="l00218"></a>00218           tmp=paj[jj];
<a name="l00219"></a>00219           paj[jj]+=d;
<a name="l00220"></a>00220           (*proj)(j, rcsubs[i], paj, pbi, hxxij, adata);
<a name="l00221"></a>00221           paj[jj]=tmp; <span class="comment">/* restore */</span>
<a name="l00222"></a>00222 
<a name="l00223"></a>00223           pAB=jac + idxij-&gt;val[rcidxs[i]]*ABsz; <span class="comment">// set pAB to point to A_ij</span>
<a name="l00224"></a>00224           <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00225"></a>00225             pAB[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1;
<a name="l00226"></a>00226         }
<a name="l00227"></a>00227       }
<a name="l00228"></a>00228     }
<a name="l00229"></a>00229 
<a name="l00230"></a>00230     <span class="comment">/* compute B_ij */</span>
<a name="l00231"></a>00231     <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l00232"></a>00232       pbi=pb+i*pnp; <span class="comment">// i-th point parameters</span>
<a name="l00233"></a>00233 
<a name="l00234"></a>00234       nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero B_ij, j=0...m-1 */</span>
<a name="l00235"></a>00235       <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l00236"></a>00236         <span class="comment">/* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */</span>
<a name="l00237"></a>00237         d=(double)(SBA_DELTA_SCALE)*pbi[jj]; <span class="comment">// force evaluation</span>
<a name="l00238"></a>00238         d=FABS(d);
<a name="l00239"></a>00239         <span class="keywordflow">if</span>(d&lt;SBA_MIN_DELTA) d=SBA_MIN_DELTA;
<a name="l00240"></a>00240         d1=1.0/d; <span class="comment">/* invert so that divisions can be carried out faster as multiplications */</span>
<a name="l00241"></a>00241 
<a name="l00242"></a>00242         <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l00243"></a>00243           paj=pa + rcsubs[j]*cnp; <span class="comment">// j-th camera parameters</span>
<a name="l00244"></a>00244           (*proj)(rcsubs[j], i, paj, pbi, hxij, adata); <span class="comment">// evaluate supplied function on current solution</span>
<a name="l00245"></a>00245 
<a name="l00246"></a>00246           tmp=pbi[jj];
<a name="l00247"></a>00247           pbi[jj]+=d;
<a name="l00248"></a>00248           (*proj)(rcsubs[j], i, paj, pbi, hxxij, adata);
<a name="l00249"></a>00249           pbi[jj]=tmp; <span class="comment">/* restore */</span>
<a name="l00250"></a>00250 
<a name="l00251"></a>00251           pAB=jac + idxij-&gt;val[rcidxs[j]]*ABsz + Asz; <span class="comment">// set pAB to point to B_ij</span>
<a name="l00252"></a>00252           <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00253"></a>00253             pAB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1;
<a name="l00254"></a>00254         }
<a name="l00255"></a>00255       }
<a name="l00256"></a>00256     }
<a name="l00257"></a>00257 
<a name="l00258"></a>00258   free(hxij);
<a name="l00259"></a>00259 }
<a name="l00260"></a>00260 
<a name="l00261"></a>00261 <span class="comment">/* BUNDLE ADJUSTMENT FOR CAMERA PARAMETERS ONLY */</span>
<a name="l00262"></a>00262 
<a name="l00263"></a>00263 <span class="comment">/* Given a parameter vector p made up of the parameters of m cameras, compute in</span>
<a name="l00264"></a>00264 <span class="comment"> * hx the prediction of the measurements, i.e. the projections of 3D points in the m images.</span>
<a name="l00265"></a>00265 <span class="comment"> * The measurements are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T,</span>
<a name="l00266"></a>00266 <span class="comment"> * where hx_ij is the predicted projection of the i-th point on the j-th camera.</span>
<a name="l00267"></a>00267 <span class="comment"> * Caller supplies rcidxs and rcsubs which can be used as working memory.</span>
<a name="l00268"></a>00268 <span class="comment"> * Notice that depending on idxij, some of the hx_ij might be missing</span>
<a name="l00269"></a>00269 <span class="comment"> *</span>
<a name="l00270"></a>00270 <span class="comment"> */</span>
<a name="l00271"></a>00271 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_mot_Qs(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata)
<a name="l00272"></a>00272 {
<a name="l00273"></a>00273   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00274"></a>00274   <span class="keywordtype">int</span> cnp, mnp; 
<a name="l00275"></a>00275   <span class="keywordtype">double</span> *paj, *pxij;
<a name="l00276"></a>00276   <span class="comment">//int n;</span>
<a name="l00277"></a>00277   <span class="keywordtype">int</span> m, nnz;
<a name="l00278"></a>00278   <span class="keyword">struct </span>wrap_mot_data_ *wdata;
<a name="l00279"></a>00279   void (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *proj_adata);
<a name="l00280"></a>00280   <span class="keywordtype">void</span> *proj_adata;
<a name="l00281"></a>00281 
<a name="l00282"></a>00282   wdata=(<span class="keyword">struct </span>wrap_mot_data_ *)adata;
<a name="l00283"></a>00283   cnp=wdata-&gt;cnp; mnp=wdata-&gt;mnp;
<a name="l00284"></a>00284   proj=wdata-&gt;proj;
<a name="l00285"></a>00285   proj_adata=wdata-&gt;adata;
<a name="l00286"></a>00286 
<a name="l00287"></a>00287   <span class="comment">//n=idxij-&gt;nr;</span>
<a name="l00288"></a>00288   m=idxij-&gt;nc;
<a name="l00289"></a>00289 
<a name="l00290"></a>00290   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00291"></a>00291     <span class="comment">/* j-th camera parameters */</span>
<a name="l00292"></a>00292     paj=p+j*cnp;
<a name="l00293"></a>00293 
<a name="l00294"></a>00294     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00295"></a>00295 
<a name="l00296"></a>00296     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00297"></a>00297       pxij=hx + idxij-&gt;val[rcidxs[i]]*mnp; <span class="comment">// set pxij to point to hx_ij</span>
<a name="l00298"></a>00298 
<a name="l00299"></a>00299       (*proj)(j, rcsubs[i], paj, pxij, proj_adata); <span class="comment">// evaluate Q in pxij</span>
<a name="l00300"></a>00300     }
<a name="l00301"></a>00301   }
<a name="l00302"></a>00302 }
<a name="l00303"></a>00303 
<a name="l00304"></a>00304 <span class="comment">/* Given a parameter vector p made up of the parameters of m cameras, compute in jac</span>
<a name="l00305"></a>00305 <span class="comment"> * the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00306"></a>00306 <span class="comment"> * The jacobian is returned in the order (A_11, ..., A_1m, ..., A_n1, ..., A_nm),</span>
<a name="l00307"></a>00307 <span class="comment"> * where A_ij=dx_ij/db_j (see HZ).</span>
<a name="l00308"></a>00308 <span class="comment"> * Caller supplies rcidxs and rcsubs which can be used as working memory.</span>
<a name="l00309"></a>00309 <span class="comment"> * Notice that depending on idxij, some of the A_ij might be missing</span>
<a name="l00310"></a>00310 <span class="comment"> *</span>
<a name="l00311"></a>00311 <span class="comment"> */</span>
<a name="l00312"></a>00312 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_mot_Qs_jac(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata)
<a name="l00313"></a>00313 {
<a name="l00314"></a>00314   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00315"></a>00315   <span class="keywordtype">int</span> cnp, mnp;
<a name="l00316"></a>00316   <span class="keywordtype">double</span> *paj, *pAij;
<a name="l00317"></a>00317   <span class="comment">//int n;</span>
<a name="l00318"></a>00318   <span class="keywordtype">int</span> m, nnz, Asz, idx;
<a name="l00319"></a>00319   <span class="keyword">struct </span>wrap_mot_data_ *wdata;
<a name="l00320"></a>00320   void (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *Aij, <span class="keywordtype">void</span> *projac_adata);
<a name="l00321"></a>00321   <span class="keywordtype">void</span> *projac_adata;
<a name="l00322"></a>00322 
<a name="l00323"></a>00323   wdata=(<span class="keyword">struct </span>wrap_mot_data_ *)adata;
<a name="l00324"></a>00324   cnp=wdata-&gt;cnp; mnp=wdata-&gt;mnp;
<a name="l00325"></a>00325   projac=wdata-&gt;projac;
<a name="l00326"></a>00326   projac_adata=wdata-&gt;adata;
<a name="l00327"></a>00327 
<a name="l00328"></a>00328   <span class="comment">//n=idxij-&gt;nr;</span>
<a name="l00329"></a>00329   m=idxij-&gt;nc;
<a name="l00330"></a>00330   Asz=mnp*cnp;
<a name="l00331"></a>00331 
<a name="l00332"></a>00332   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00333"></a>00333     <span class="comment">/* j-th camera parameters */</span>
<a name="l00334"></a>00334     paj=p+j*cnp;
<a name="l00335"></a>00335 
<a name="l00336"></a>00336     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00337"></a>00337 
<a name="l00338"></a>00338     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00339"></a>00339       idx=idxij-&gt;val[rcidxs[i]];
<a name="l00340"></a>00340       pAij=jac + idx*Asz; <span class="comment">// set pAij to point to A_ij</span>
<a name="l00341"></a>00341 
<a name="l00342"></a>00342       (*projac)(j, rcsubs[i], paj, pAij, projac_adata); <span class="comment">// evaluate dQ/da in pAij</span>
<a name="l00343"></a>00343     }
<a name="l00344"></a>00344   }
<a name="l00345"></a>00345 }
<a name="l00346"></a>00346 
<a name="l00347"></a>00347 <span class="comment">/* Given a parameter vector p made up of the parameters of m cameras, compute in jac the jacobian</span>
<a name="l00348"></a>00348 <span class="comment"> * of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00349"></a>00349 <span class="comment"> * The jacobian is approximated with the aid of finite differences and is returned in the order</span>
<a name="l00350"></a>00350 <span class="comment"> * (A_11, ..., A_1m, ..., A_n1, ..., A_nm), where A_ij=dx_ij/da_j (see HZ).</span>
<a name="l00351"></a>00351 <span class="comment"> * Notice that depending on idxij, some of the A_ij might be missing</span>
<a name="l00352"></a>00352 <span class="comment"> *</span>
<a name="l00353"></a>00353 <span class="comment"> * Problem-specific information is assumed to be stored in a structure pointed to by &quot;dat&quot;.</span>
<a name="l00354"></a>00354 <span class="comment"> *</span>
<a name="l00355"></a>00355 <span class="comment"> * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,</span>
<a name="l00356"></a>00356 <span class="comment"> * the jacobian should be computed analytically</span>
<a name="l00357"></a>00357 <span class="comment"> */</span>
<a name="l00358"></a>00358 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_mot_Qs_fdjac(
<a name="l00359"></a>00359     <span class="keywordtype">double</span> *p,                <span class="comment">/* I: current parameter estimate, (m*cnp)x1 */</span>
<a name="l00360"></a>00360     <span class="keyword">struct</span> sba_crsm *idxij,   <span class="comment">/* I: sparse matrix containing the location of x_ij in hx */</span>
<a name="l00361"></a>00361     <span class="keywordtype">int</span>    *rcidxs,           <span class="comment">/* work array for the indexes of nonzero elements of a single sparse matrix row/column */</span>
<a name="l00362"></a>00362     <span class="keywordtype">int</span>    *rcsubs,           <span class="comment">/* work array for the subscripts of nonzero elements in a single sparse matrix row/column */</span>
<a name="l00363"></a>00363     <span class="keywordtype">double</span> *jac,              <span class="comment">/* O: array for storing the approximated jacobian */</span>
<a name="l00364"></a>00364     <span class="keywordtype">void</span>   *dat)              <span class="comment">/* I: points to a &quot;wrap_mot_data_&quot; structure */</span>
<a name="l00365"></a>00365 {
<a name="l00366"></a>00366   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj;
<a name="l00367"></a>00367   <span class="keywordtype">double</span> *paj;
<a name="l00368"></a>00368   <span class="keyword">register</span> <span class="keywordtype">double</span> *pA;
<a name="l00369"></a>00369   <span class="comment">//int n; </span>
<a name="l00370"></a>00370   <span class="keywordtype">int</span> m, nnz, Asz;
<a name="l00371"></a>00371 
<a name="l00372"></a>00372   <span class="keywordtype">double</span> tmp;
<a name="l00373"></a>00373   <span class="keyword">register</span> <span class="keywordtype">double</span> d, d1;
<a name="l00374"></a>00374 
<a name="l00375"></a>00375   <span class="keyword">struct </span>wrap_mot_data_ *fdjd;
<a name="l00376"></a>00376   void (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata);
<a name="l00377"></a>00377   <span class="keywordtype">double</span> *hxij, *hxxij;
<a name="l00378"></a>00378   <span class="keywordtype">int</span> cnp, mnp;
<a name="l00379"></a>00379   <span class="keywordtype">void</span> *adata;
<a name="l00380"></a>00380 
<a name="l00381"></a>00381   <span class="comment">/* retrieve problem-specific information passed in *dat */</span>
<a name="l00382"></a>00382   fdjd=(<span class="keyword">struct </span>wrap_mot_data_ *)dat;
<a name="l00383"></a>00383   proj=fdjd-&gt;proj;
<a name="l00384"></a>00384   cnp=fdjd-&gt;cnp; mnp=fdjd-&gt;mnp;
<a name="l00385"></a>00385   adata=fdjd-&gt;adata;
<a name="l00386"></a>00386 
<a name="l00387"></a>00387   <span class="comment">//n=idxij-&gt;nr;</span>
<a name="l00388"></a>00388   m=idxij-&gt;nc;
<a name="l00389"></a>00389   Asz=mnp*cnp;
<a name="l00390"></a>00390 
<a name="l00391"></a>00391   <span class="comment">/* allocate memory for hxij, hxxij */</span>
<a name="l00392"></a>00392   <span class="keywordflow">if</span>((hxij=malloc(2*mnp*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>)))==NULL){
<a name="l00393"></a>00393     fprintf(stderr, <span class="stringliteral">&quot;memory allocation request failed in sba_mot_Qs_fdjac()!\n&quot;</span>);
<a name="l00394"></a>00394     exit(1);
<a name="l00395"></a>00395   }
<a name="l00396"></a>00396   hxxij=hxij+mnp;
<a name="l00397"></a>00397 
<a name="l00398"></a>00398   <span class="comment">/* compute A_ij */</span>
<a name="l00399"></a>00399   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00400"></a>00400     paj=p+j*cnp; <span class="comment">// j-th camera parameters</span>
<a name="l00401"></a>00401 
<a name="l00402"></a>00402     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero A_ij, i=0...n-1 */</span>
<a name="l00403"></a>00403     <span class="keywordflow">for</span>(jj=0; jj&lt;cnp; ++jj){
<a name="l00404"></a>00404       <span class="comment">/* determine d=max(SBA_DELTA_SCALE*|paj[jj]|, SBA_MIN_DELTA), see HZ */</span>
<a name="l00405"></a>00405       d=(double)(SBA_DELTA_SCALE)*paj[jj]; <span class="comment">// force evaluation</span>
<a name="l00406"></a>00406       d=FABS(d);
<a name="l00407"></a>00407       <span class="keywordflow">if</span>(d&lt;SBA_MIN_DELTA) d=SBA_MIN_DELTA;
<a name="l00408"></a>00408       d1=1.0/d; <span class="comment">/* invert so that divisions can be carried out faster as multiplications */</span>
<a name="l00409"></a>00409 
<a name="l00410"></a>00410       <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00411"></a>00411         (*proj)(j, rcsubs[i], paj, hxij, adata); <span class="comment">// evaluate supplied function on current solution</span>
<a name="l00412"></a>00412 
<a name="l00413"></a>00413         tmp=paj[jj];
<a name="l00414"></a>00414         paj[jj]+=d;
<a name="l00415"></a>00415         (*proj)(j, rcsubs[i], paj, hxxij, adata);
<a name="l00416"></a>00416         paj[jj]=tmp; <span class="comment">/* restore */</span>
<a name="l00417"></a>00417 
<a name="l00418"></a>00418         pA=jac + idxij-&gt;val[rcidxs[i]]*Asz; <span class="comment">// set pA to point to A_ij</span>
<a name="l00419"></a>00419         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00420"></a>00420           pA[ii*cnp+jj]=(hxxij[ii]-hxij[ii])*d1;
<a name="l00421"></a>00421       }
<a name="l00422"></a>00422     }
<a name="l00423"></a>00423   }
<a name="l00424"></a>00424 
<a name="l00425"></a>00425   free(hxij);
<a name="l00426"></a>00426 }
<a name="l00427"></a>00427 
<a name="l00428"></a>00428 <span class="comment">/* BUNDLE ADJUSTMENT FOR STRUCTURE PARAMETERS ONLY */</span>
<a name="l00429"></a>00429 
<a name="l00430"></a>00430 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points, compute in</span>
<a name="l00431"></a>00431 <span class="comment"> * hx the prediction of the measurements, i.e. the projections of 3D points in the m images. The measurements</span>
<a name="l00432"></a>00432 <span class="comment"> * are returned in the order (hx_11^T, .. hx_1m^T, ..., hx_n1^T, .. hx_nm^T)^T, where hx_ij is the predicted</span>
<a name="l00433"></a>00433 <span class="comment"> * projection of the i-th point on the j-th camera.</span>
<a name="l00434"></a>00434 <span class="comment"> * Caller supplies rcidxs and rcsubs which can be used as working memory.</span>
<a name="l00435"></a>00435 <span class="comment"> * Notice that depending on idxij, some of the hx_ij might be missing</span>
<a name="l00436"></a>00436 <span class="comment"> *</span>
<a name="l00437"></a>00437 <span class="comment"> */</span>
<a name="l00438"></a>00438 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_str_Qs(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *hx, <span class="keywordtype">void</span> *adata)
<a name="l00439"></a>00439 {
<a name="l00440"></a>00440   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00441"></a>00441   <span class="keywordtype">int</span> pnp, mnp; 
<a name="l00442"></a>00442   <span class="keywordtype">double</span> *pbi, *pxij;
<a name="l00443"></a>00443   <span class="comment">//int n;</span>
<a name="l00444"></a>00444   <span class="keywordtype">int</span> m, nnz;
<a name="l00445"></a>00445   <span class="keyword">struct </span>wrap_str_data_ *wdata;
<a name="l00446"></a>00446   void (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *proj_adata);
<a name="l00447"></a>00447   <span class="keywordtype">void</span> *proj_adata;
<a name="l00448"></a>00448 
<a name="l00449"></a>00449   wdata=(<span class="keyword">struct </span>wrap_str_data_ *)adata;
<a name="l00450"></a>00450   pnp=wdata-&gt;pnp; mnp=wdata-&gt;mnp;
<a name="l00451"></a>00451   proj=wdata-&gt;proj;
<a name="l00452"></a>00452   proj_adata=wdata-&gt;adata;
<a name="l00453"></a>00453 
<a name="l00454"></a>00454   <span class="comment">//n=idxij-&gt;nr;</span>
<a name="l00455"></a>00455   m=idxij-&gt;nc;
<a name="l00456"></a>00456 
<a name="l00457"></a>00457   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00458"></a>00458     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00459"></a>00459 
<a name="l00460"></a>00460     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00461"></a>00461       pbi=p + rcsubs[i]*pnp;
<a name="l00462"></a>00462       pxij=hx + idxij-&gt;val[rcidxs[i]]*mnp; <span class="comment">// set pxij to point to hx_ij</span>
<a name="l00463"></a>00463 
<a name="l00464"></a>00464       (*proj)(j, rcsubs[i], pbi, pxij, proj_adata); <span class="comment">// evaluate Q in pxij</span>
<a name="l00465"></a>00465     }
<a name="l00466"></a>00466   }
<a name="l00467"></a>00467 }
<a name="l00468"></a>00468 
<a name="l00469"></a>00469 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points, compute in</span>
<a name="l00470"></a>00470 <span class="comment"> * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00471"></a>00471 <span class="comment"> * The jacobian is returned in the order (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ).</span>
<a name="l00472"></a>00472 <span class="comment"> * Caller supplies rcidxs and rcsubs which can be used as working memory.</span>
<a name="l00473"></a>00473 <span class="comment"> * Notice that depending on idxij, some of the B_ij might be missing</span>
<a name="l00474"></a>00474 <span class="comment"> *</span>
<a name="l00475"></a>00475 <span class="comment"> */</span>
<a name="l00476"></a>00476 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_str_Qs_jac(<span class="keywordtype">double</span> *p, <span class="keyword">struct</span> sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata)
<a name="l00477"></a>00477 {
<a name="l00478"></a>00478   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j;
<a name="l00479"></a>00479   <span class="keywordtype">int</span> pnp, mnp;
<a name="l00480"></a>00480   <span class="keywordtype">double</span> *pbi, *pBij;
<a name="l00481"></a>00481   <span class="comment">//int n;</span>
<a name="l00482"></a>00482   <span class="keywordtype">int</span> m, nnz, Bsz, idx;
<a name="l00483"></a>00483   <span class="keyword">struct </span>wrap_str_data_ *wdata;
<a name="l00484"></a>00484   void (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *Bij, <span class="keywordtype">void</span> *projac_adata);
<a name="l00485"></a>00485   <span class="keywordtype">void</span> *projac_adata;
<a name="l00486"></a>00486 
<a name="l00487"></a>00487   
<a name="l00488"></a>00488   wdata=(<span class="keyword">struct </span>wrap_str_data_ *)adata;
<a name="l00489"></a>00489   pnp=wdata-&gt;pnp; mnp=wdata-&gt;mnp;
<a name="l00490"></a>00490   projac=wdata-&gt;projac;
<a name="l00491"></a>00491   projac_adata=wdata-&gt;adata;
<a name="l00492"></a>00492 
<a name="l00493"></a>00493   <span class="comment">//n=idxij-&gt;nr;</span>
<a name="l00494"></a>00494   m=idxij-&gt;nc;
<a name="l00495"></a>00495   Bsz=mnp*pnp;
<a name="l00496"></a>00496 
<a name="l00497"></a>00497   <span class="keywordflow">for</span>(j=0; j&lt;m; ++j){
<a name="l00498"></a>00498 
<a name="l00499"></a>00499     nnz=sba_crsm_col_elmidxs(idxij, j, rcidxs, rcsubs); <span class="comment">/* find nonzero hx_ij, i=0...n-1 */</span>
<a name="l00500"></a>00500 
<a name="l00501"></a>00501     <span class="keywordflow">for</span>(i=0; i&lt;nnz; ++i){
<a name="l00502"></a>00502       pbi=p + rcsubs[i]*pnp;
<a name="l00503"></a>00503       idx=idxij-&gt;val[rcidxs[i]];
<a name="l00504"></a>00504       pBij=jac + idx*Bsz; <span class="comment">// set pBij to point to B_ij</span>
<a name="l00505"></a>00505 
<a name="l00506"></a>00506       (*projac)(j, rcsubs[i], pbi, pBij, projac_adata); <span class="comment">// evaluate dQ/db in pBij</span>
<a name="l00507"></a>00507     }
<a name="l00508"></a>00508   }
<a name="l00509"></a>00509 }
<a name="l00510"></a>00510 
<a name="l00511"></a>00511 <span class="comment">/* Given a parameter vector p made up of the 3D coordinates of n points, compute in</span>
<a name="l00512"></a>00512 <span class="comment"> * jac the jacobian of the predicted measurements, i.e. the jacobian of the projections of 3D points in the m images.</span>
<a name="l00513"></a>00513 <span class="comment"> * The jacobian is approximated with the aid of finite differences and is returned in the order</span>
<a name="l00514"></a>00514 <span class="comment"> * (B_11, ..., B_1m, ..., B_n1, ..., B_nm), where B_ij=dx_ij/db_i (see HZ).</span>
<a name="l00515"></a>00515 <span class="comment"> * Notice that depending on idxij, some of the B_ij might be missing</span>
<a name="l00516"></a>00516 <span class="comment"> *</span>
<a name="l00517"></a>00517 <span class="comment"> * Problem-specific information is assumed to be stored in a structure pointed to by &quot;dat&quot;.</span>
<a name="l00518"></a>00518 <span class="comment"> *</span>
<a name="l00519"></a>00519 <span class="comment"> * NOTE: This function is provided mainly for illustration purposes; in case that execution time is a concern,</span>
<a name="l00520"></a>00520 <span class="comment"> * the jacobian should be computed analytically</span>
<a name="l00521"></a>00521 <span class="comment"> */</span>
<a name="l00522"></a>00522 <span class="keyword">static</span> <span class="keywordtype">void</span> sba_str_Qs_fdjac(
<a name="l00523"></a>00523     <span class="keywordtype">double</span> *p,                <span class="comment">/* I: current parameter estimate, (n*pnp)x1 */</span>
<a name="l00524"></a>00524     <span class="keyword">struct</span> sba_crsm *idxij,   <span class="comment">/* I: sparse matrix containing the location of x_ij in hx */</span>
<a name="l00525"></a>00525     <span class="keywordtype">int</span>    *rcidxs,           <span class="comment">/* work array for the indexes of nonzero elements of a single sparse matrix row/column */</span>
<a name="l00526"></a>00526     <span class="keywordtype">int</span>    *rcsubs,           <span class="comment">/* work array for the subscripts of nonzero elements in a single sparse matrix row/column */</span>
<a name="l00527"></a>00527     <span class="keywordtype">double</span> *jac,              <span class="comment">/* O: array for storing the approximated jacobian */</span>
<a name="l00528"></a>00528     <span class="keywordtype">void</span>   *dat)              <span class="comment">/* I: points to a &quot;wrap_str_data_&quot; structure */</span>
<a name="l00529"></a>00529 {
<a name="l00530"></a>00530   <span class="keyword">register</span> <span class="keywordtype">int</span> i, j, ii, jj;
<a name="l00531"></a>00531   <span class="keywordtype">double</span> *pbi;
<a name="l00532"></a>00532   <span class="keyword">register</span> <span class="keywordtype">double</span> *pB;
<a name="l00533"></a>00533   <span class="comment">//int m;</span>
<a name="l00534"></a>00534   <span class="keywordtype">int</span> n, nnz, Bsz;
<a name="l00535"></a>00535 
<a name="l00536"></a>00536   <span class="keywordtype">double</span> tmp;
<a name="l00537"></a>00537   <span class="keyword">register</span> <span class="keywordtype">double</span> d, d1;
<a name="l00538"></a>00538 
<a name="l00539"></a>00539   <span class="keyword">struct </span>wrap_str_data_ *fdjd;
<a name="l00540"></a>00540   void (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata);
<a name="l00541"></a>00541   <span class="keywordtype">double</span> *hxij, *hxxij;
<a name="l00542"></a>00542   <span class="keywordtype">int</span> pnp, mnp;
<a name="l00543"></a>00543   <span class="keywordtype">void</span> *adata;
<a name="l00544"></a>00544 
<a name="l00545"></a>00545   <span class="comment">/* retrieve problem-specific information passed in *dat */</span>
<a name="l00546"></a>00546   fdjd=(<span class="keyword">struct </span>wrap_str_data_ *)dat;
<a name="l00547"></a>00547   proj=fdjd-&gt;proj;
<a name="l00548"></a>00548   pnp=fdjd-&gt;pnp; mnp=fdjd-&gt;mnp;
<a name="l00549"></a>00549   adata=fdjd-&gt;adata;
<a name="l00550"></a>00550 
<a name="l00551"></a>00551   n=idxij-&gt;nr;
<a name="l00552"></a>00552   <span class="comment">//m=idxij-&gt;nc;</span>
<a name="l00553"></a>00553   Bsz=mnp*pnp;
<a name="l00554"></a>00554 
<a name="l00555"></a>00555   <span class="comment">/* allocate memory for hxij, hxxij */</span>
<a name="l00556"></a>00556   <span class="keywordflow">if</span>((hxij=malloc(2*mnp*<span class="keyword">sizeof</span>(<span class="keywordtype">double</span>)))==NULL){
<a name="l00557"></a>00557     fprintf(stderr, <span class="stringliteral">&quot;memory allocation request failed in sba_str_Qs_fdjac()!\n&quot;</span>);
<a name="l00558"></a>00558     exit(1);
<a name="l00559"></a>00559   }
<a name="l00560"></a>00560   hxxij=hxij+mnp;
<a name="l00561"></a>00561 
<a name="l00562"></a>00562   <span class="comment">/* compute B_ij */</span>
<a name="l00563"></a>00563   <span class="keywordflow">for</span>(i=0; i&lt;n; ++i){
<a name="l00564"></a>00564     pbi=p+i*pnp; <span class="comment">// i-th point parameters</span>
<a name="l00565"></a>00565 
<a name="l00566"></a>00566     nnz=sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); <span class="comment">/* find nonzero B_ij, j=0...m-1 */</span>
<a name="l00567"></a>00567     <span class="keywordflow">for</span>(jj=0; jj&lt;pnp; ++jj){
<a name="l00568"></a>00568       <span class="comment">/* determine d=max(SBA_DELTA_SCALE*|pbi[jj]|, SBA_MIN_DELTA), see HZ */</span>
<a name="l00569"></a>00569       d=(double)(SBA_DELTA_SCALE)*pbi[jj]; <span class="comment">// force evaluation</span>
<a name="l00570"></a>00570       d=FABS(d);
<a name="l00571"></a>00571       <span class="keywordflow">if</span>(d&lt;SBA_MIN_DELTA) d=SBA_MIN_DELTA;
<a name="l00572"></a>00572       d1=1.0/d; <span class="comment">/* invert so that divisions can be carried out faster as multiplications */</span>
<a name="l00573"></a>00573 
<a name="l00574"></a>00574       <span class="keywordflow">for</span>(j=0; j&lt;nnz; ++j){
<a name="l00575"></a>00575         (*proj)(rcsubs[j], i, pbi, hxij, adata); <span class="comment">// evaluate supplied function on current solution</span>
<a name="l00576"></a>00576 
<a name="l00577"></a>00577         tmp=pbi[jj];
<a name="l00578"></a>00578         pbi[jj]+=d;
<a name="l00579"></a>00579         (*proj)(rcsubs[j], i, pbi, hxxij, adata);
<a name="l00580"></a>00580         pbi[jj]=tmp; <span class="comment">/* restore */</span>
<a name="l00581"></a>00581 
<a name="l00582"></a>00582         pB=jac + idxij-&gt;val[rcidxs[j]]*Bsz; <span class="comment">// set pB to point to B_ij</span>
<a name="l00583"></a>00583         <span class="keywordflow">for</span>(ii=0; ii&lt;mnp; ++ii)
<a name="l00584"></a>00584           pB[ii*pnp+jj]=(hxxij[ii]-hxij[ii])*d1;
<a name="l00585"></a>00585       }
<a name="l00586"></a>00586     }
<a name="l00587"></a>00587   }
<a name="l00588"></a>00588 
<a name="l00589"></a>00589   free(hxij);
<a name="l00590"></a>00590 }
<a name="l00591"></a>00591 
<a name="l00592"></a>00592 
<a name="l00593"></a>00593 <span class="comment">/* </span>
<a name="l00594"></a>00594 <span class="comment"> * Simple driver to sba_motstr_levmar_x for bundle adjustment on camera and structure parameters.</span>
<a name="l00595"></a>00595 <span class="comment"> *</span>
<a name="l00596"></a>00596 <span class="comment"> * Returns the number of iterations (&gt;=0) if successfull, SBA_ERROR if failed</span>
<a name="l00597"></a>00597 <span class="comment"> */</span>
<a name="l00598"></a>00598 
<a name="l00599"></a>00599 <span class="keywordtype">int</span> sba_motstr_levmar(
<a name="l00600"></a>00600     <span class="keyword">const</span> <span class="keywordtype">int</span> n,   <span class="comment">/* number of points */</span>
<a name="l00601"></a>00601     <span class="keyword">const</span> <span class="keywordtype">int</span> ncon,<span class="comment">/* number of points (starting from the 1st) whose parameters should not be modified.</span>
<a name="l00602"></a>00602 <span class="comment">                    * All B_ij (see below) with i&lt;ncon are assumed to be zero</span>
<a name="l00603"></a>00603 <span class="comment">                    */</span>
<a name="l00604"></a>00604     <span class="keyword">const</span> <span class="keywordtype">int</span> m,   <span class="comment">/* number of images */</span>
<a name="l00605"></a>00605     <span class="keyword">const</span> <span class="keywordtype">int</span> mcon,<span class="comment">/* number of images (starting from the 1st) whose parameters should not be modified.</span>
<a name="l00606"></a>00606 <span class="comment">                                                  * All A_ij (see below) with j&lt;mcon are assumed to be zero</span>
<a name="l00607"></a>00607 <span class="comment">                                                  */</span>
<a name="l00608"></a>00608     <span class="keywordtype">char</span> *vmask,  <span class="comment">/* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */</span>
<a name="l00609"></a>00609     <span class="keywordtype">double</span> *p,    <span class="comment">/* initial parameter vector p0: (a1, ..., am, b1, ..., bn).</span>
<a name="l00610"></a>00610 <span class="comment">                   * aj are the image j parameters, bi are the i-th point parameters,</span>
<a name="l00611"></a>00611 <span class="comment">                   * size m*cnp + n*pnp</span>
<a name="l00612"></a>00612 <span class="comment">                   */</span>
<a name="l00613"></a>00613     <span class="keyword">const</span> <span class="keywordtype">int</span> cnp,<span class="comment">/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */</span>
<a name="l00614"></a>00614     <span class="keyword">const</span> <span class="keywordtype">int</span> pnp,<span class="comment">/* number of parameters for ONE point; e.g. 3 for Euclidean points */</span>
<a name="l00615"></a>00615     <span class="keywordtype">double</span> *x,    <span class="comment">/* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where</span>
<a name="l00616"></a>00616 <span class="comment">                   * x_ij is the projection of the i-th point on the j-th image.</span>
<a name="l00617"></a>00617 <span class="comment">                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;</span>
<a name="l00618"></a>00618 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp</span>
<a name="l00619"></a>00619 <span class="comment">                   */</span>
<a name="l00620"></a>00620     <span class="keywordtype">double</span> *covx, <span class="comment">/* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm),</span>
<a name="l00621"></a>00621 <span class="comment">                   * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no</span>
<a name="l00622"></a>00622 <span class="comment">                   * covariance estimates are available (identity matrices are implicitly used in this case).</span>
<a name="l00623"></a>00623 <span class="comment">                   * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing;</span>
<a name="l00624"></a>00624 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp*mnp</span>
<a name="l00625"></a>00625 <span class="comment">                   */</span>
<a name="l00626"></a>00626     <span class="keyword">const</span> <span class="keywordtype">int</span> mnp,<span class="comment">/* number of parameters for EACH measurement; usually 2 */</span>
<a name="l00627"></a>00627     <span class="keywordtype">void</span> (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata),
<a name="l00628"></a>00628                                               <span class="comment">/* functional relation computing a SINGLE image measurement. Assuming that</span>
<a name="l00629"></a>00629 <span class="comment">                                               * the parameters of point i are bi and the parameters of camera j aj,</span>
<a name="l00630"></a>00630 <span class="comment">                                               * computes a prediction of \hat{x}_{ij}. aj is cnp x 1, bi is pnp x 1 and</span>
<a name="l00631"></a>00631 <span class="comment">                                               * xij is mnp x 1. This function is called only if point i is visible in</span>
<a name="l00632"></a>00632 <span class="comment">                                               * image j (i.e. vmask[i, j]==1)</span>
<a name="l00633"></a>00633 <span class="comment">                                               */</span>
<a name="l00634"></a>00634     <span class="keywordtype">void</span> (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *Aij, <span class="keywordtype">double</span> *Bij, <span class="keywordtype">void</span> *adata),
<a name="l00635"></a>00635                                               <span class="comment">/* functional relation to evaluate d x_ij / d a_j and</span>
<a name="l00636"></a>00636 <span class="comment">                                               * d x_ij / d b_i in Aij and Bij resp.</span>
<a name="l00637"></a>00637 <span class="comment">                                               * This function is called only if point i is visible in * image j</span>
<a name="l00638"></a>00638 <span class="comment">                                               * (i.e. vmask[i, j]==1). Also, A_ij and B_ij are mnp x cnp and mnp x pnp</span>
<a name="l00639"></a>00639 <span class="comment">                                               * matrices resp. and they should be stored in row-major order.</span>
<a name="l00640"></a>00640 <span class="comment">                                               *</span>
<a name="l00641"></a>00641 <span class="comment">                                               * If NULL, the jacobians are approximated by repetitive proj calls</span>
<a name="l00642"></a>00642 <span class="comment">                                               * and finite differences. </span>
<a name="l00643"></a>00643 <span class="comment">                                               */</span>
<a name="l00644"></a>00644     <span class="keywordtype">void</span> *adata,       <span class="comment">/* pointer to possibly additional data, passed uninterpreted to proj, projac */</span> 
<a name="l00645"></a>00645 
<a name="l00646"></a>00646     <span class="keyword">const</span> <span class="keywordtype">int</span> itmax,   <span class="comment">/* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */</span>
<a name="l00647"></a>00647     <span class="keyword">const</span> <span class="keywordtype">int</span> verbose, <span class="comment">/* I: verbosity */</span>
<a name="l00648"></a>00648     <span class="keyword">const</span> <span class="keywordtype">double</span> opts[SBA_OPTSSZ],
<a name="l00649"></a>00649                              <span class="comment">/* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu,</span>
<a name="l00650"></a>00650 <span class="comment">                        * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2</span>
<a name="l00651"></a>00651 <span class="comment">                        */</span>
<a name="l00652"></a>00652     <span class="keywordtype">double</span> info[SBA_INFOSZ]
<a name="l00653"></a>00653                              <span class="comment">/* O: information regarding the minimization. Set to NULL if don&#39;t care</span>
<a name="l00654"></a>00654 <span class="comment">                        * info[0]=||e||_2 at initial p.</span>
<a name="l00655"></a>00655 <span class="comment">                        * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l00656"></a>00656 <span class="comment">                        * info[5]= # iterations,</span>
<a name="l00657"></a>00657 <span class="comment">                        * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l00658"></a>00658 <span class="comment">                        *                                 2 - stopped by small dp</span>
<a name="l00659"></a>00659 <span class="comment">                        *                                 3 - stopped by itmax</span>
<a name="l00660"></a>00660 <span class="comment">                        *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l00661"></a>00661 <span class="comment">                        *                                 5 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l00662"></a>00662 <span class="comment">                        *                                 6 - stopped by small ||e||_2</span>
<a name="l00663"></a>00663 <span class="comment">                        *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values. This is a user error</span>
<a name="l00664"></a>00664 <span class="comment">                        * info[7]= # function evaluations</span>
<a name="l00665"></a>00665 <span class="comment">                        * info[8]= # jacobian evaluations</span>
<a name="l00666"></a>00666 <span class="comment">                                          * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l00667"></a>00667 <span class="comment">                        */</span>
<a name="l00668"></a>00668 )
<a name="l00669"></a>00669 {
<a name="l00670"></a>00670 <span class="keywordtype">int</span> retval;
<a name="l00671"></a>00671 <span class="keyword">struct </span>wrap_motstr_data_ wdata;
<a name="l00672"></a>00672 <span class="keyword">static</span> void (*fjac)(<span class="keywordtype">double</span> *p, <span class="keyword">struct </span>sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata);
<a name="l00673"></a>00673 
<a name="l00674"></a>00674   wdata.proj=proj;
<a name="l00675"></a>00675   wdata.projac=projac;
<a name="l00676"></a>00676   wdata.cnp=cnp;
<a name="l00677"></a>00677   wdata.pnp=pnp;
<a name="l00678"></a>00678   wdata.mnp=mnp;
<a name="l00679"></a>00679   wdata.adata=adata;
<a name="l00680"></a>00680 
<a name="l00681"></a>00681   fjac=(projac)? sba_motstr_Qs_jac : sba_motstr_Qs_fdjac;
<a name="l00682"></a>00682   retval=sba_motstr_levmar_x(n, ncon, m, mcon, vmask, p, cnp, pnp, x, covx, mnp, sba_motstr_Qs, fjac, &amp;wdata, itmax, verbose, opts, info);
<a name="l00683"></a>00683 
<a name="l00684"></a>00684   <span class="keywordflow">if</span>(info){
<a name="l00685"></a>00685     <span class="keyword">register</span> <span class="keywordtype">int</span> i;
<a name="l00686"></a>00686     <span class="keywordtype">int</span> nvis;
<a name="l00687"></a>00687 
<a name="l00688"></a>00688     <span class="comment">/* count visible image points */</span>
<a name="l00689"></a>00689     <span class="keywordflow">for</span>(i=nvis=0; i&lt;n*m; ++i)
<a name="l00690"></a>00690       nvis+=(vmask[i]!=0);
<a name="l00691"></a>00691 
<a name="l00692"></a>00692     <span class="comment">/* each &quot;func&quot; &amp; &quot;fjac&quot; evaluation requires nvis &quot;proj&quot; &amp; &quot;projac&quot; evaluations */</span>
<a name="l00693"></a>00693     info[7]*=nvis;
<a name="l00694"></a>00694     info[8]*=nvis;
<a name="l00695"></a>00695   }
<a name="l00696"></a>00696 
<a name="l00697"></a>00697   <span class="keywordflow">return</span> retval;
<a name="l00698"></a>00698 }
<a name="l00699"></a>00699 
<a name="l00700"></a>00700 
<a name="l00701"></a>00701 <span class="comment">/* </span>
<a name="l00702"></a>00702 <span class="comment"> * Simple driver to sba_mot_levmar_x for bundle adjustment on camera parameters.</span>
<a name="l00703"></a>00703 <span class="comment"> *</span>
<a name="l00704"></a>00704 <span class="comment"> * Returns the number of iterations (&gt;=0) if successfull, SBA_ERROR if failed</span>
<a name="l00705"></a>00705 <span class="comment"> */</span>
<a name="l00706"></a>00706 
<a name="l00707"></a>00707 <span class="keywordtype">int</span> sba_mot_levmar(
<a name="l00708"></a>00708     <span class="keyword">const</span> <span class="keywordtype">int</span> n,   <span class="comment">/* number of points */</span>
<a name="l00709"></a>00709     <span class="keyword">const</span> <span class="keywordtype">int</span> m,   <span class="comment">/* number of images */</span>
<a name="l00710"></a>00710     <span class="keyword">const</span> <span class="keywordtype">int</span> mcon,<span class="comment">/* number of images (starting from the 1st) whose parameters should not be modified.</span>
<a name="l00711"></a>00711 <span class="comment">                                                  * All A_ij (see below) with j&lt;mcon are assumed to be zero</span>
<a name="l00712"></a>00712 <span class="comment">                                                  */</span>
<a name="l00713"></a>00713     <span class="keywordtype">char</span> *vmask,  <span class="comment">/* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */</span>
<a name="l00714"></a>00714     <span class="keywordtype">double</span> *p,    <span class="comment">/* initial parameter vector p0: (a1, ..., am).</span>
<a name="l00715"></a>00715 <span class="comment">                   * aj are the image j parameters, size m*cnp */</span>
<a name="l00716"></a>00716     <span class="keyword">const</span> <span class="keywordtype">int</span> cnp,<span class="comment">/* number of parameters for ONE camera; e.g. 6 for Euclidean cameras */</span>
<a name="l00717"></a>00717     <span class="keywordtype">double</span> *x,    <span class="comment">/* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where</span>
<a name="l00718"></a>00718 <span class="comment">                   * x_ij is the projection of the i-th point on the j-th image.</span>
<a name="l00719"></a>00719 <span class="comment">                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;</span>
<a name="l00720"></a>00720 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp</span>
<a name="l00721"></a>00721 <span class="comment">                   */</span>
<a name="l00722"></a>00722     <span class="keywordtype">double</span> *covx, <span class="comment">/* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm),</span>
<a name="l00723"></a>00723 <span class="comment">                   * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no</span>
<a name="l00724"></a>00724 <span class="comment">                   * covariance estimates are available (identity matrices are implicitly used in this case).</span>
<a name="l00725"></a>00725 <span class="comment">                   * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing;</span>
<a name="l00726"></a>00726 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp*mnp</span>
<a name="l00727"></a>00727 <span class="comment">                   */</span>
<a name="l00728"></a>00728     <span class="keyword">const</span> <span class="keywordtype">int</span> mnp,<span class="comment">/* number of parameters for EACH measurement; usually 2 */</span>
<a name="l00729"></a>00729     <span class="keywordtype">void</span> (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata),
<a name="l00730"></a>00730                                               <span class="comment">/* functional relation computing a SINGLE image measurement. Assuming that</span>
<a name="l00731"></a>00731 <span class="comment">                                               * the parameters of camera j are aj, computes a prediction of \hat{x}_{ij}</span>
<a name="l00732"></a>00732 <span class="comment">                                               * for point i. aj is cnp x 1 and xij is mnp x 1.</span>
<a name="l00733"></a>00733 <span class="comment">                                               * This function is called only if point i is visible in  image j (i.e. vmask[i, j]==1)</span>
<a name="l00734"></a>00734 <span class="comment">                                               */</span>
<a name="l00735"></a>00735     <span class="keywordtype">void</span> (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *aj, <span class="keywordtype">double</span> *Aij, <span class="keywordtype">void</span> *adata),
<a name="l00736"></a>00736                                               <span class="comment">/* functional relation to evaluate d x_ij / d a_j in Aij </span>
<a name="l00737"></a>00737 <span class="comment">                                               * This function is called only if point i is visible in image j</span>
<a name="l00738"></a>00738 <span class="comment">                                               * (i.e. vmask[i, j]==1). Also, A_ij are a mnp x cnp matrices</span>
<a name="l00739"></a>00739 <span class="comment">                                               * and should be stored in row-major order.</span>
<a name="l00740"></a>00740 <span class="comment">                                               *</span>
<a name="l00741"></a>00741 <span class="comment">                                               * If NULL, the jacobian is approximated by repetitive proj calls</span>
<a name="l00742"></a>00742 <span class="comment">                                               * and finite differences. </span>
<a name="l00743"></a>00743 <span class="comment">                                               */</span>
<a name="l00744"></a>00744     <span class="keywordtype">void</span> *adata,       <span class="comment">/* pointer to possibly additional data, passed uninterpreted to proj, projac */</span> 
<a name="l00745"></a>00745 
<a name="l00746"></a>00746     <span class="keyword">const</span> <span class="keywordtype">int</span> itmax,   <span class="comment">/* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */</span>
<a name="l00747"></a>00747     <span class="keyword">const</span> <span class="keywordtype">int</span> verbose, <span class="comment">/* I: verbosity */</span>
<a name="l00748"></a>00748     <span class="keyword">const</span> <span class="keywordtype">double</span> opts[SBA_OPTSSZ],
<a name="l00749"></a>00749                              <span class="comment">/* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon]. Respectively the scale factor for initial \mu,</span>
<a name="l00750"></a>00750 <span class="comment">                        * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2</span>
<a name="l00751"></a>00751 <span class="comment">                        */</span>
<a name="l00752"></a>00752     <span class="keywordtype">double</span> info[SBA_INFOSZ]
<a name="l00753"></a>00753                              <span class="comment">/* O: information regarding the minimization. Set to NULL if don&#39;t care</span>
<a name="l00754"></a>00754 <span class="comment">                        * info[0]=||e||_2 at initial p.</span>
<a name="l00755"></a>00755 <span class="comment">                        * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l00756"></a>00756 <span class="comment">                        * info[5]= # iterations,</span>
<a name="l00757"></a>00757 <span class="comment">                        * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l00758"></a>00758 <span class="comment">                        *                                 2 - stopped by small dp</span>
<a name="l00759"></a>00759 <span class="comment">                        *                                 3 - stopped by itmax</span>
<a name="l00760"></a>00760 <span class="comment">                        *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l00761"></a>00761 <span class="comment">                        *                                 5 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l00762"></a>00762 <span class="comment">                        *                                 6 - stopped by small ||e||_2</span>
<a name="l00763"></a>00763 <span class="comment">                        *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values. This is a user error</span>
<a name="l00764"></a>00764 <span class="comment">                        * info[7]= # function evaluations</span>
<a name="l00765"></a>00765 <span class="comment">                        * info[8]= # jacobian evaluations</span>
<a name="l00766"></a>00766 <span class="comment">                                          * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l00767"></a>00767 <span class="comment">                        */</span>
<a name="l00768"></a>00768 )
<a name="l00769"></a>00769 {
<a name="l00770"></a>00770 <span class="keywordtype">int</span> retval;
<a name="l00771"></a>00771 <span class="keyword">struct </span>wrap_mot_data_ wdata;
<a name="l00772"></a>00772 void (*fjac)(<span class="keywordtype">double</span> *p, <span class="keyword">struct </span>sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata);
<a name="l00773"></a>00773 
<a name="l00774"></a>00774   wdata.proj=proj;
<a name="l00775"></a>00775   wdata.projac=projac;
<a name="l00776"></a>00776   wdata.cnp=cnp;
<a name="l00777"></a>00777   wdata.mnp=mnp;
<a name="l00778"></a>00778   wdata.adata=adata;
<a name="l00779"></a>00779 
<a name="l00780"></a>00780   fjac=(projac)? sba_mot_Qs_jac : sba_mot_Qs_fdjac;
<a name="l00781"></a>00781   retval=sba_mot_levmar_x(n, m, mcon, vmask, p, cnp, x, covx, mnp, sba_mot_Qs, fjac, &amp;wdata, itmax, verbose, opts, info);
<a name="l00782"></a>00782 
<a name="l00783"></a>00783   <span class="keywordflow">if</span>(info){
<a name="l00784"></a>00784     <span class="keyword">register</span> <span class="keywordtype">int</span> i;
<a name="l00785"></a>00785     <span class="keywordtype">int</span> nvis;
<a name="l00786"></a>00786 
<a name="l00787"></a>00787     <span class="comment">/* count visible image points */</span>
<a name="l00788"></a>00788     <span class="keywordflow">for</span>(i=nvis=0; i&lt;n*m; ++i)
<a name="l00789"></a>00789       nvis+=(vmask[i]!=0);
<a name="l00790"></a>00790 
<a name="l00791"></a>00791     <span class="comment">/* each &quot;func&quot; &amp; &quot;fjac&quot; evaluation requires nvis &quot;proj&quot; &amp; &quot;projac&quot; evaluations */</span>
<a name="l00792"></a>00792     info[7]*=nvis;
<a name="l00793"></a>00793     info[8]*=nvis;
<a name="l00794"></a>00794   }
<a name="l00795"></a>00795 
<a name="l00796"></a>00796   <span class="keywordflow">return</span> retval;
<a name="l00797"></a>00797 }
<a name="l00798"></a>00798 
<a name="l00799"></a>00799 <span class="comment">/* </span>
<a name="l00800"></a>00800 <span class="comment"> * Simple driver to sba_str_levmar_x for bundle adjustment on structure parameters.</span>
<a name="l00801"></a>00801 <span class="comment"> *</span>
<a name="l00802"></a>00802 <span class="comment"> * Returns the number of iterations (&gt;=0) if successfull, SBA_ERROR if failed</span>
<a name="l00803"></a>00803 <span class="comment"> */</span>
<a name="l00804"></a>00804 
<a name="l00805"></a>00805 <span class="keywordtype">int</span> sba_str_levmar(
<a name="l00806"></a>00806     <span class="keyword">const</span> <span class="keywordtype">int</span> n,   <span class="comment">/* number of points */</span>
<a name="l00807"></a>00807     <span class="keyword">const</span> <span class="keywordtype">int</span> ncon,<span class="comment">/* number of points (starting from the 1st) whose parameters should not be modified.</span>
<a name="l00808"></a>00808 <span class="comment">                    * All B_ij (see below) with i&lt;ncon are assumed to be zero</span>
<a name="l00809"></a>00809 <span class="comment">                    */</span>
<a name="l00810"></a>00810     <span class="keyword">const</span> <span class="keywordtype">int</span> m,   <span class="comment">/* number of images */</span>
<a name="l00811"></a>00811     <span class="keywordtype">char</span> *vmask,  <span class="comment">/* visibility mask: vmask[i, j]=1 if point i visible in image j, 0 otherwise. nxm */</span>
<a name="l00812"></a>00812     <span class="keywordtype">double</span> *p,    <span class="comment">/* initial parameter vector p0: (b1, ..., bn).</span>
<a name="l00813"></a>00813 <span class="comment">                   * bi are the i-th point parameters, size n*pnp</span>
<a name="l00814"></a>00814 <span class="comment">                   */</span>
<a name="l00815"></a>00815     <span class="keyword">const</span> <span class="keywordtype">int</span> pnp,<span class="comment">/* number of parameters for ONE point; e.g. 3 for Euclidean points */</span>
<a name="l00816"></a>00816     <span class="keywordtype">double</span> *x,    <span class="comment">/* measurements vector: (x_11^T, .. x_1m^T, ..., x_n1^T, .. x_nm^T)^T where</span>
<a name="l00817"></a>00817 <span class="comment">                   * x_ij is the projection of the i-th point on the j-th image.</span>
<a name="l00818"></a>00818 <span class="comment">                   * NOTE: some of the x_ij might be missing, if point i is not visible in image j;</span>
<a name="l00819"></a>00819 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp</span>
<a name="l00820"></a>00820 <span class="comment">                   */</span>
<a name="l00821"></a>00821     <span class="keywordtype">double</span> *covx, <span class="comment">/* measurements covariance matrices: (Sigma_x_11, .. Sigma_x_1m, ..., Sigma_x_n1, .. Sigma_x_nm),</span>
<a name="l00822"></a>00822 <span class="comment">                   * where Sigma_x_ij is the mnp x mnp covariance of x_ij stored row-by-row. Set to NULL if no</span>
<a name="l00823"></a>00823 <span class="comment">                   * covariance estimates are available (identity matrices are implicitly used in this case).</span>
<a name="l00824"></a>00824 <span class="comment">                   * NOTE: a certain Sigma_x_ij is missing if the corresponding x_ij is also missing;</span>
<a name="l00825"></a>00825 <span class="comment">                   * see vmask[i, j], max. size n*m*mnp*mnp</span>
<a name="l00826"></a>00826 <span class="comment">                   */</span>
<a name="l00827"></a>00827     <span class="keyword">const</span> <span class="keywordtype">int</span> mnp,<span class="comment">/* number of parameters for EACH measurement; usually 2 */</span>
<a name="l00828"></a>00828     <span class="keywordtype">void</span> (*proj)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *xij, <span class="keywordtype">void</span> *adata),
<a name="l00829"></a>00829                                               <span class="comment">/* functional relation computing a SINGLE image measurement. Assuming that</span>
<a name="l00830"></a>00830 <span class="comment">                                               * the parameters of point i are bi, computes a prediction of \hat{x}_{ij}.</span>
<a name="l00831"></a>00831 <span class="comment">                                               * bi is pnp x 1 and  xij is mnp x 1. This function is called only if point</span>
<a name="l00832"></a>00832 <span class="comment">                                               * i is visible in image j (i.e. vmask[i, j]==1)</span>
<a name="l00833"></a>00833 <span class="comment">                                               */</span>
<a name="l00834"></a>00834     <span class="keywordtype">void</span> (*projac)(<span class="keywordtype">int</span> j, <span class="keywordtype">int</span> i, <span class="keywordtype">double</span> *bi, <span class="keywordtype">double</span> *Bij, <span class="keywordtype">void</span> *adata),
<a name="l00835"></a>00835                                               <span class="comment">/* functional relation to evaluate d x_ij / d b_i in Bij.</span>
<a name="l00836"></a>00836 <span class="comment">                                               * This function is called only if point i is visible in image j</span>
<a name="l00837"></a>00837 <span class="comment">                                               * (i.e. vmask[i, j]==1). Also, B_ij are mnp x pnp matrices</span>
<a name="l00838"></a>00838 <span class="comment">                                               * and they should be stored in row-major order.</span>
<a name="l00839"></a>00839 <span class="comment">                                               *</span>
<a name="l00840"></a>00840 <span class="comment">                                               * If NULL, the jacobians are approximated by repetitive proj calls</span>
<a name="l00841"></a>00841 <span class="comment">                                               * and finite differences. </span>
<a name="l00842"></a>00842 <span class="comment">                                               */</span>
<a name="l00843"></a>00843     <span class="keywordtype">void</span> *adata,       <span class="comment">/* pointer to possibly additional data, passed uninterpreted to proj, projac */</span> 
<a name="l00844"></a>00844 
<a name="l00845"></a>00845     <span class="keyword">const</span> <span class="keywordtype">int</span> itmax,   <span class="comment">/* I: maximum number of iterations. itmax==0 signals jacobian verification followed by immediate return */</span>
<a name="l00846"></a>00846     <span class="keyword">const</span> <span class="keywordtype">int</span> verbose, <span class="comment">/* I: verbosity */</span>
<a name="l00847"></a>00847     <span class="keyword">const</span> <span class="keywordtype">double</span> opts[SBA_OPTSSZ],
<a name="l00848"></a>00848                              <span class="comment">/* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3, \epsilon4]. Respectively the scale factor for initial \mu,</span>
<a name="l00849"></a>00849 <span class="comment">                        * stoping thresholds for ||J^T e||_inf, ||dp||_2, ||e||_2 and (||e||_2-||e_new||_2)/||e||_2</span>
<a name="l00850"></a>00850 <span class="comment">                        */</span>
<a name="l00851"></a>00851     <span class="keywordtype">double</span> info[SBA_INFOSZ]
<a name="l00852"></a>00852                              <span class="comment">/* O: information regarding the minimization. Set to NULL if don&#39;t care</span>
<a name="l00853"></a>00853 <span class="comment">                        * info[0]=||e||_2 at initial p.</span>
<a name="l00854"></a>00854 <span class="comment">                        * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l00855"></a>00855 <span class="comment">                        * info[5]= # iterations,</span>
<a name="l00856"></a>00856 <span class="comment">                        * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l00857"></a>00857 <span class="comment">                        *                                 2 - stopped by small dp</span>
<a name="l00858"></a>00858 <span class="comment">                        *                                 3 - stopped by itmax</span>
<a name="l00859"></a>00859 <span class="comment">                        *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l00860"></a>00860 <span class="comment">                        *                                 5 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l00861"></a>00861 <span class="comment">                        *                                 6 - stopped by small ||e||_2</span>
<a name="l00862"></a>00862 <span class="comment">                        *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values. This is a user error</span>
<a name="l00863"></a>00863 <span class="comment">                        * info[7]= # function evaluations</span>
<a name="l00864"></a>00864 <span class="comment">                        * info[8]= # jacobian evaluations</span>
<a name="l00865"></a>00865 <span class="comment">                                          * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l00866"></a>00866 <span class="comment">                        */</span>
<a name="l00867"></a>00867 )
<a name="l00868"></a>00868 {
<a name="l00869"></a>00869 <span class="keywordtype">int</span> retval;
<a name="l00870"></a>00870 <span class="keyword">struct </span>wrap_str_data_ wdata;
<a name="l00871"></a>00871 <span class="keyword">static</span> void (*fjac)(<span class="keywordtype">double</span> *p, <span class="keyword">struct </span>sba_crsm *idxij, <span class="keywordtype">int</span> *rcidxs, <span class="keywordtype">int</span> *rcsubs, <span class="keywordtype">double</span> *jac, <span class="keywordtype">void</span> *adata);
<a name="l00872"></a>00872 
<a name="l00873"></a>00873   wdata.proj=proj;
<a name="l00874"></a>00874   wdata.projac=projac;
<a name="l00875"></a>00875   wdata.pnp=pnp;
<a name="l00876"></a>00876   wdata.mnp=mnp;
<a name="l00877"></a>00877   wdata.adata=adata;
<a name="l00878"></a>00878 
<a name="l00879"></a>00879   fjac=(projac)? sba_str_Qs_jac : sba_str_Qs_fdjac;
<a name="l00880"></a>00880   retval=sba_str_levmar_x(n, ncon, m, vmask, p, pnp, x, covx, mnp, sba_str_Qs, fjac, &amp;wdata, itmax, verbose, opts, info);
<a name="l00881"></a>00881 
<a name="l00882"></a>00882   <span class="keywordflow">if</span>(info){
<a name="l00883"></a>00883     <span class="keyword">register</span> <span class="keywordtype">int</span> i;
<a name="l00884"></a>00884     <span class="keywordtype">int</span> nvis;
<a name="l00885"></a>00885 
<a name="l00886"></a>00886     <span class="comment">/* count visible image points */</span>
<a name="l00887"></a>00887     <span class="keywordflow">for</span>(i=nvis=0; i&lt;n*m; ++i)
<a name="l00888"></a>00888       nvis+=(vmask[i]!=0);
<a name="l00889"></a>00889 
<a name="l00890"></a>00890     <span class="comment">/* each &quot;func&quot; &amp; &quot;fjac&quot; evaluation requires nvis &quot;proj&quot; &amp; &quot;projac&quot; evaluations */</span>
<a name="l00891"></a>00891     info[7]*=nvis;
<a name="l00892"></a>00892     info[8]*=nvis;
<a name="l00893"></a>00893   }
<a name="l00894"></a>00894 
<a name="l00895"></a>00895   <span class="keywordflow">return</span> retval;
<a name="l00896"></a>00896 }
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
